﻿/*
 * SM2 Encryption alogrithm
 *
 * References:
 * - GM/T 0003-2012 Chinese National Standard:
 *      Public Key Cryptographic Algorithm SM2 Based on Elliptic Curves
 * - GM/T 0009-2012 SM2 cryptography algorithm application specification
 * - GM/T 0015-2012 Digital certificate format based on SM2 algorithm
 *
 * Thanks to MbedTLS.
 */

#if !defined(MBEDTLS_CONFIG_FILE)
#include "mbedtls/config.h"
#else
#include MBEDTLS_CONFIG_FILE
#endif

#if defined(MBEDTLS_SM2_C)

#include "mbedtls/ssm2.h"
#include "mbedtls/sm3.h"
#include "mbedtls/entropy.h"
#include "mbedtls/ctr_drbg.h"

#if defined(MBEDTLS_PLATFORM_C)
#include "mbedtls/platform.h"
#else
#include <stdio.h>
#include <stdlib.h>

#define mbedtls_calloc      calloc
#define mbedtls_printf      printf
#define mbedtls_free        free
#endif /* MBEDTLS_PLATFORM_C */

/* ssm2 utils { */
#define SM3_DIGEST_LENGTH 32

static void dump_buf( const char *title, unsigned char *buf, size_t len )
{
  size_t i;

  mbedtls_printf( "%s", title );
  for( i = 0; i < len; i++ )
    mbedtls_printf("%c%c", "0123456789ABCDEF" [buf[i] / 16],
        "0123456789ABCDEF" [buf[i] % 16] );
  mbedtls_printf( "\n" );
}

static void dump_mpi( const char* title, const mbedtls_mpi *num )
{
  unsigned char buf[300];
  size_t len = mbedtls_mpi_size(num);

  if( mbedtls_mpi_write_binary( num, buf, len)!=0)
  {
    mbedtls_printf("internal error\n");
    return;
  }

  dump_buf( title, buf, len );
}

static void dump_point( const char* title,
    const mbedtls_ecp_group *grp,
    const mbedtls_ecp_point *pnt )
{
  unsigned char buf[300];
  size_t len;

  if( mbedtls_ecp_point_write_binary( grp, pnt, MBEDTLS_ECP_PF_UNCOMPRESSED, &len, buf, sizeof buf ) != 0 )
  {
    mbedtls_printf("internal error\n");
    return;
  }

  dump_buf( title, buf, len );
}

static void dump_pubkey( const char *title, const mbedtls_sm2_context *key )
{
  dump_point(title, &key->grp, &key->Q);
}

/**
 * Get random r in [1, n-1]
 */
//static 
int sm2_get_rand(const mbedtls_ecp_group *grp, mbedtls_mpi *r,
    int (*f_rng)(void *, unsigned char *, size_t), void *p_rng)
{
  int ret;
  size_t blind_tries = 0;
  do
  {
    size_t nlen = (grp->nbits + 7) / 8;
    MBEDTLS_MPI_CHK(mbedtls_mpi_fill_random(r, nlen, f_rng, p_rng));
    MBEDTLS_MPI_CHK(mbedtls_mpi_shift_r(r, 8 * nlen - grp->nbits));

    /* See mbedtls_ecp_gen_keypair() */
    if (++blind_tries > 30)
      return (MBEDTLS_ERR_SM2_RANDOM_FAILED);
  }
  while (mbedtls_mpi_cmp_int(r, 1) < 0 ||
      mbedtls_mpi_cmp_mpi(r, &grp->N) >= 0);
cleanup:
  return (ret);
}

/**
 * SM2 KDF (ISO/IEC 15946-2 3.1.3)
 * (GM/T 0003-2012 - Part 3: Key Exchange Protocol 5.4.3)
 */
//static 
int mbedtls_sm2_pbkdf2(mbedtls_md_context_t *ctx,
    const unsigned char *password, size_t plen,
    const unsigned char *salt, size_t slen,
    unsigned int iteration_count,
    uint32_t key_length, unsigned char *output)
{
  int ret, j;
  unsigned int i;
  unsigned char md1[MBEDTLS_MD_MAX_SIZE];
  unsigned char work[MBEDTLS_MD_MAX_SIZE];
  unsigned char md_size = mbedtls_md_get_size(ctx->md_info);
  size_t use_len;
  unsigned char *out_p = output;
  unsigned char counter[4];

  memset(counter, 0, 4);
  counter[3] = 1;

  if (iteration_count > 0xFFFFFFFF)
    return (MBEDTLS_ERR_SM2_BAD_INPUT_DATA);

  while (key_length)
  {
    /* U1 ends up in work */
    if ((ret = mbedtls_md_starts(ctx)) != 0)
      return ret;
    if ((ret = mbedtls_md_update(ctx, password, plen)) != 0)
      return ret;
    if ((ret = mbedtls_md_update(ctx, salt, slen)) != 0)
      return ret;
    if ((ret = mbedtls_md_update(ctx, counter, 4)) != 0)
      return ret;
    if ((ret = mbedtls_md_finish(ctx, work)) != 0)
      return ret;

    memcpy(md1, work, md_size);

    for (i = 1; i < iteration_count; i++)
    {
      /* U2 ends up in md1 */
      if ((ret = mbedtls_md_hmac_starts(ctx, password, plen)) != 0)
        return (ret);
      if ((ret = mbedtls_md_hmac_update(ctx, md1, md_size)) != 0)
        return (ret);
      if ((ret = mbedtls_md_hmac_finish(ctx, md1)) != 0)
        return (ret);

      /* U1 xor U2 */
      for (j = 0; j < md_size; j++)
        work[j] ^= md1[j];
    }

    use_len = (key_length < md_size) ? key_length : md_size;
    memcpy(out_p, work, use_len);

    key_length -= (uint32_t)use_len;
    out_p += use_len;

    for (i = 4; i > 0; i--)
      if (++counter[i - 1] != 0)
        break;
  }

  return (0);
}

/*
   int mbedtls_calc_z(const unsigned char* id, size_t idlen,
   const mbedtls_ecp_point *pubkey,
   unsigned char* md[SM3_DIGEST_LENGTH])
   {
   int entl;
   unsigned char b[2];
   mbedtls_sm3_context ctx;
   unsigned char buf[SM3_DIGEST_LENGTH];

   if (mbedtls_mpi_size(&pubkey->X)!=32 ||
   mbedtls_mpi_size(&pubkey->Y)!=32 ||
   mbedtls_mpi_size(&pubkey->G))
   entl = idlen * 8;
   b[0] = (entl >> 8) % 256;
   b[1] = (entl >> 0) % 256;
   mbedtls_sm3_init(&ctx);
   mbedtls_sm3_update(&ctx, b, 2);
   mbedtls_sm3_update(&ctx, (unsigned char*)id, idlen);
   mbedtls_mpi_write_binary()
   int mbedtls_mpi_write_binary( const mbedtls_mpi *X, unsigned char *buf, size_t buflen )
   mbedtls_sm3_update(&ctx, sm2a, 32);
   mbedtls_sm3_update(&ctx, sm2b, 32);
   mbedtls_sm3_update(&ctx, sm2Gx, 32);
   mbedtls_sm3_update(&ctx, sm2Gy, 32);
   if(pubkey_len == 65 && pubkey[0] == 0x04)
   sm3_update(&ctx, &pubkey[1], 64);
   else
   sm3_update(&ctx, &pubkey[0], 64);
   sm3_final(&ctx, md);

   return 1;
   }
   */
int mbedtls_sm2_get_Z(mbedtls_ecp_group *grp, mbedtls_ecp_point *P, unsigned char * ID, unsigned int IDbitlen, unsigned char *Z)
{
	unsigned char tmpdata[512];
	mbedtls_sm3_context sm3_ctx;
	unsigned char *p = tmpdata;
	int ret = 0, len = 0;;
	p[0] = IDbitlen / 256;
	p[1] = IDbitlen % 256;
	p += 2;
	len += 2;
	if (ID != NULL)
	{
		memcpy(p, ID, IDbitlen / 8);
		p += IDbitlen / 8;
		len += IDbitlen / 8;
	}
	MBEDTLS_MPI_CHK(mbedtls_mpi_write_binary(&grp->A, p, 32));
	p += 32;
	len += 32;

	MBEDTLS_MPI_CHK(mbedtls_mpi_write_binary(&grp->B, p, 32));
	p += 32;
	len += 32;

	MBEDTLS_MPI_CHK(mbedtls_mpi_write_binary(&grp->G.X, p, 32));
	p += 32;
	len += 32;

	MBEDTLS_MPI_CHK(mbedtls_mpi_write_binary(&grp->G.Y, p, 32));
	p += 32;
	len += 32;

	MBEDTLS_MPI_CHK(mbedtls_mpi_write_binary(&P->X, p, 32));
	p += 32;
	len += 32;

	MBEDTLS_MPI_CHK(mbedtls_mpi_write_binary(&P->Y, p, 32));
	len += 32;

	mbedtls_sm3_starts(&sm3_ctx);
	mbedtls_sm3_update(&sm3_ctx, tmpdata, len);
	mbedtls_sm3_finish(&sm3_ctx, Z);

cleanup:
	return ret;
}
int mbedtls_sm2_KDF(const unsigned char* cdata, int datalen, int keylen, unsigned char* retdata)
{
	int nRet = -1;
	unsigned char *pRet;
	unsigned char *pData;
	unsigned char RetData[256];
	unsigned char Data[256];
	unsigned char cdgst[32] = { 0 }; //摘要
	unsigned char cCnt[4] = { 0 }; //计数器的内存表示值
	int nCnt = 1;  //计数器
	int nDgst = 32; //摘要长度
	mbedtls_sm3_context sm3_ctx;
	int nTimes = (keylen + 31) / 32; //需要计算的次数
	int i = 0;

	pData = Data;
	pRet = RetData;
	if (cdata == NULL || datalen <= 0 || keylen <= 0)
	{
		goto err;
	}

	/*	if(NULL == (pRet=(unsigned char *)malloc(keylen)))
	{
	goto err;
	}

	if(NULL == (pData=(unsigned char *)malloc(datalen+4)))
	{
	goto err;
	}*/

	memset(pRet, 0, keylen);
	memset(pData, 0, datalen + 4);

	memcpy(pData, cdata, datalen);
	for (i = 0; i<nTimes; i++)
	{
		//cCnt
		{
			cCnt[0] = (nCnt >> 24) & 0xFF;
			cCnt[1] = (nCnt >> 16) & 0xFF;
			cCnt[2] = (nCnt >> 8) & 0xFF;
			cCnt[3] = (nCnt)& 0xFF;
		}
		memcpy(pData + datalen, cCnt, 4);
		mbedtls_sm3_starts(&sm3_ctx);
		mbedtls_sm3_update(&sm3_ctx, pData, datalen + 4);
		mbedtls_sm3_finish(&sm3_ctx, cdgst);

		if (i == nTimes - 1) //最后一次计算，根据keylen/32是否整除，截取摘要的值
		{
			if (keylen % 32 != 0)
			{
				nDgst = keylen % 32;
			}
		}
		memcpy(pRet + 32 * i, cdgst, nDgst);

		//i    ++;  //
		nCnt++;  //
	}

	if (retdata != NULL)
	{
		memcpy(retdata, pRet, keylen);
	}

	nRet = 0;
err:
	

	return nRet;
}

/* } ssm2 utils */

/* ssm2 decrypt { */
int mbedtls_ssm2_decrypt_step1(mbedtls_ecp_group *grp, const mbedtls_mpi *d1,
    const mbedtls_ecp_point *C1, mbedtls_ecp_point *T1)
{
  int ret;
  //T1＝D1[*]C1
  ret = mbedtls_ecp_mul(grp, T1, d1, C1, NULL, NULL);
  return ret;
}

// (x2, y2) = d[*]C1
int mbedtls_ssm2_decrypt_step2(mbedtls_ecp_group *grp, const mbedtls_mpi *d2,
    const mbedtls_ecp_point *T1, mbedtls_ecp_point *T2)
{
  int ret;
  mbedtls_mpi inv;
  mbedtls_mpi_init(&inv);
  //T2＝D2^-1[*]T1
  ret = mbedtls_mpi_inv_mod(&inv, d2, &grp->N);
  if (ret!=0)
    goto cleanup;
  ret = mbedtls_ecp_mul(grp, T2, &inv, T1, NULL, NULL);
cleanup:
  mbedtls_mpi_free(&inv);
  return ret;
}

int mbedtls_ssm2_decrypt_step3(mbedtls_ecp_group *grp, const mbedtls_ecp_point *C1,
    const mbedtls_ecp_point *T2, mbedtls_ecp_point *P)
{
  int ret;

  mbedtls_mpi one;
  mbedtls_mpi done;
  mbedtls_mpi_init(&one);
  mbedtls_mpi_init(&done);

  // Q = (x2，y2)＝T2[-]C1
  //   = (D2^-1*T1)-C1
  //   = (D2^-1 * D1*C1) [-]C1
  //   = ((D2^-1)*D1-1) [-] C1
  // D = (D2^-1)*D1-1

  mbedtls_mpi_lset(&one, 1);
  mbedtls_mpi_lset(&done, -1);

  ret = mbedtls_ecp_muladd(grp, P, &one, T2, &done, C1 );
  if (ret!=0)
  {
    goto cleanup;
  }

cleanup:
  mbedtls_mpi_free(&one);
  mbedtls_mpi_free(&done);

  return ret;
}

int mbedtls_ssm2_decrypt(mbedtls_sm2_context *ctxc, mbedtls_sm2_context *ctxs,
    mbedtls_md_type_t md_alg,
    const unsigned char *input, size_t ilen,
    unsigned char *output, size_t *olen)
{
  int ret = 0;
  size_t i;
  mbedtls_mpi h;
  mbedtls_ecp_point C1;
  mbedtls_ecp_point T1;
  mbedtls_ecp_point T2;
  mbedtls_ecp_point point;
  mbedtls_md_context_t md_ctx;
  size_t c1len, ptlen, mdlen, xlen, ylen;
  unsigned char *xym = NULL;

  mbedtls_ecp_point_init(&C1);
  mbedtls_ecp_point_init(&T1);
  mbedtls_ecp_point_init(&T2);
  mbedtls_ecp_point_init(&point);
  mbedtls_mpi_init(&h);
  mbedtls_md_init(&md_ctx);
  MBEDTLS_MPI_CHK(mbedtls_md_setup(&md_ctx, mbedtls_md_info_from_type(md_alg), 0));

  /* B1: get C1 */
  c1len = 1 + (ctxc->grp.nbits + 7) / 8 * 2;
  MBEDTLS_MPI_CHK(mbedtls_ecp_point_read_binary(&ctxc->grp, &C1, input, c1len));

  //1. T1＝D1^-1[*]C1
  MBEDTLS_MPI_CHK(mbedtls_ssm2_decrypt_step1(&ctxc->grp, &ctxc->d, &C1, &T1));

  //2. T2＝D2[*]T1
  MBEDTLS_MPI_CHK(mbedtls_ssm2_decrypt_step2(&ctxc->grp, &ctxs->d, &T1, &T2));

  // Q = (x2，y2)＝T2[-]C1。
  MBEDTLS_MPI_CHK(mbedtls_ssm2_decrypt_step3(&ctxc->grp, &C1, &T2, &point));

  //
  // t＝KDF(x2||y2，klen)
  // 若t等于0，否则，可报错退出

  /* B4: t = KDF(x2 || y2, klen) */
  xlen = mbedtls_mpi_size(&point.X);
  ylen = mbedtls_mpi_size(&point.Y);
  mdlen = mbedtls_md_get_size(md_ctx.md_info);
  ptlen = ilen - c1len - mdlen;
  if ((xym = mbedtls_calloc(1, xlen + ylen + ptlen)) == NULL)
  {
    MBEDTLS_MPI_CHK(MBEDTLS_ERR_SM2_ALLOC_FAILED);
  }
  MBEDTLS_MPI_CHK(mbedtls_mpi_write_binary(&point.X, xym, xlen));
  MBEDTLS_MPI_CHK(mbedtls_mpi_write_binary(&point.Y, xym + xlen, ylen));
  MBEDTLS_MPI_CHK(mbedtls_sm2_pbkdf2(&md_ctx, xym, xlen + ylen,
        NULL, 0, 0, ptlen, xym + xlen + ylen));
  for (i = 0; i < ptlen; i++)
  {
    if (*(xym + xlen + ylen + i))
    {
      break;
    }
  }
  if (i >= xlen + ylen)
  {
    MBEDTLS_MPI_CHK(MBEDTLS_ERR_SM2_KDF_FAILED);
  }

  /* B5: M' = C2 xor t */
  for (i = 0; i < ptlen; i++)
  {
    output[i] = input[c1len + i] ^ *(xym + xlen + ylen + i);
  }
  *olen = ptlen;

  /* B6: check Hash(x2 || M' || y2) == C3 */
  MBEDTLS_MPI_CHK(mbedtls_mpi_write_binary(&point.X, xym, xlen));
  memmove(xym + xlen, output, *olen);
  MBEDTLS_MPI_CHK(mbedtls_mpi_write_binary(&point.Y,
        xym + xlen + *olen, ylen));
  MBEDTLS_MPI_CHK(mbedtls_md(md_ctx.md_info, xym, xlen + *olen + ylen, xym));
  if (memcmp(input + c1len + ptlen, xym, mdlen))
  {
    MBEDTLS_MPI_CHK(MBEDTLS_ERR_SM2_DECRYPT_BAD_HASH);
  }

cleanup:
  mbedtls_mpi_free(&h);
  mbedtls_ecp_point_free(&point);
  mbedtls_ecp_point_free(&C1);
  mbedtls_ecp_point_free(&T1);
  mbedtls_ecp_point_free(&T2);
  if (xym)
  {
    mbedtls_free(xym);
  }
  mbedtls_md_free(&md_ctx);

  return (ret);
}
/* } ssm2 decrypt */

/* ssm2 sign{ */
/*
## 协同签名流程如下：

1. 通讯A端生成随机数d1∈[1，n-1]，计算P1 = d1[*]G，作为子公钥1上传通讯B端。
2. 通讯B端生成随机数d2∈[1，n-1]，计算P = d2-1[*]P1[-]G，并作为公钥分发。
3. 通讯A端生成随机数k1∈[1，n-1]，计算R1 = k1[*]G，计算e=Hash(m),其中m是待签名的消息, Hash为单向哈希函数(SM3)，将R1、e上传通讯B端。
4. 通讯B端计算步骤
生成随机数k2∈[1，n-1]，计算R2 = k2[*]G。
生成随机数k3∈[1，n-1]，计算(x1，y1) = k3[*]R1[+]R2。
计算r = (x1+e)mod n，如果r = 0，则重新选取随机数k3计算。
计算s2 = d2*k3mod n。
计算s3 = d2*(r+k2)mod n。
将r、s2、s3传到通讯A端。
5. 通讯A端计算s = (d1-1*k1)*s2+d1-1*s3-r mod n，如果s = 0 或者s + r = n，则重新选取随机数k1，从步骤3开始计算。
6. r,s为消息m的签名值
*/

//3. 通讯A端生成随机数k1∈[1，n-1]，计算R1 = k1[*]G，计算e=Hash(m),其中m是待签名的消息, Hash为单向哈希函数(SM3)，将R1、e上传通讯B端。
//static 
int mbedtls_ssm2_sign_step1(const mbedtls_ecp_group *grp,
    mbedtls_mpi *k1, mbedtls_ecp_point *q1,
    int (*f_rng)(void *, unsigned char *, size_t), void *p_rng)
{
  int ret;

  // k1∈[1，n-1]
  MBEDTLS_MPI_CHK( sm2_get_rand(grp,  k1, f_rng, p_rng ) );

  // R1＝k1[*]G
  ret = mbedtls_ecp_mul( (mbedtls_ecp_group*)grp, q1, k1, &grp->G, f_rng, p_rng );
cleanup:
  return ret;
}

/*
   4. 通讯B端计算步骤
   生成随机数k2∈[1，n-1]，计算R2 = k2[*]G。
   生成随机数k3∈[1，n-1]，计算(x1，y1) = k3[*]R1[+]R2。
   计算r = (x1+e)mod n，如果r = 0，则重新选取随机数k3计算。
   计算s2 = d2*k3mod n。
   计算s3 = d2*(r+k2)mod n。
   将r、s2、s3传到通讯A端
   */
//(B)第二步

int mbedtls_ssm2_sign_step2(
    mbedtls_ecp_group *grp,
    const mbedtls_mpi *d2,        // 服务器端私钥成分
    const mbedtls_ecp_point *q1,  // 客户端签名结果
    const mbedtls_mpi *e,         // 客户端摘要
    mbedtls_mpi *r,
    mbedtls_mpi *s2,
    mbedtls_mpi *s3,
    int (*f_rng)(void *, unsigned char *, size_t), void *p_rng)
{
  int	ret = -1;

  mbedtls_mpi one;
  mbedtls_mpi k2;
  mbedtls_mpi k3;

  mbedtls_ecp_point q2;

  mbedtls_mpi_init(&k2);
  mbedtls_mpi_init(&k3);
  mbedtls_mpi_init(&one);
  mbedtls_mpi_lset(&one, 1);
  mbedtls_ecp_point_init(&q2);

  // k2∈[1，n-1]
  MBEDTLS_MPI_CHK( sm2_get_rand(grp, &k2, f_rng, p_rng ) );

  // q2＝k2[*]G
  ret = mbedtls_ecp_mul( grp, &q2, &k2, &grp->G, f_rng, p_rng );
  if(ret!=0)
    goto cleanup;

  do {
    // k3∈[1，n-1]
    MBEDTLS_MPI_CHK( sm2_get_rand(grp, &k3, f_rng, p_rng ) );
    // (x1，y1)＝k3[*]q1[+]q2
    // points by integers: R = m * P + n * Q
    // int mbedtls_ecp_muladd( mbedtls_ecp_group *grp, mbedtls_ecp_point *R,
    //           const mbedtls_mpi *m, const mbedtls_ecp_point *P,
    //           const mbedtls_mpi *n, const mbedtls_ecp_point *Q );

    ret = mbedtls_ecp_muladd( grp, &q2, &k3, q1, &one, &q2 );
    if(ret!=0)
      goto cleanup;
    // r＝x1+e mod n
    ret = mbedtls_mpi_add_mpi( r, &q2.X, e );
    if (ret!=0)
      goto cleanup;
    ret = mbedtls_mpi_mod_mpi( r, r, &grp->N );
    if (ret!=0)
      goto cleanup;
  } while (mbedtls_mpi_cmp_int(r, 0) == 0); //r != 0

  // s2＝d2*k3 mod n
  ret = mbedtls_mpi_mul_mpi(s2, d2, &k3);
  if (ret!=0)
    goto cleanup;
  ret = mbedtls_mpi_mod_mpi(s2, s2, &grp->N);
  if (ret!=0)
    goto cleanup;

  // s3＝d2*(r+k2) mod n
  ret = mbedtls_mpi_add_mpi(s3, r, &k2);
  if (ret!=0)
    goto cleanup;
  ret = mbedtls_mpi_mod_mpi(s3, s3, &grp->N);
  if (ret!=0)
    goto cleanup;
  ret = mbedtls_mpi_mul_mpi(s3, s3, d2);
  if (ret!=0)
    goto cleanup;
  ret = mbedtls_mpi_mod_mpi(s3, s3, &grp->N);
cleanup:

  mbedtls_mpi_free( &one );
  mbedtls_mpi_free( &k2 );
  mbedtls_mpi_free( &k3 );

  mbedtls_ecp_point_free( &q2 );

  return ret;
}

// 5. 通讯A端计算s = (d1-1*k1)*s2+d1-1*s3-r mod n，如果s = 0 或者s + r = n，则重新选取随机数k1，从步骤3开始计算。
int mbedtls_ssm2_sign_step3(const mbedtls_ecp_group* grp,
    const mbedtls_mpi *d1, const mbedtls_mpi *k1,
    const mbedtls_mpi *r, const mbedtls_mpi *s2, const mbedtls_mpi *s3,
    mbedtls_mpi *s)
{
  int ret;
  mbedtls_mpi d_inv;
  mbedtls_mpi t_i1;
  mbedtls_mpi t_i2;

  mbedtls_mpi_init(&d_inv);
  mbedtls_mpi_init(&t_i1);
  mbedtls_mpi_init(&t_i2);

  // s＝(d1^-1*k1)s2+d1^-1*s3-r mod n
  //  = d_inv * k1 * s2 + d_inv * s3 -r mod n

  // d_inv = d1^-1
  //* Modular inverse: X = A^-1 mod N  (HAC 14.61 / 14.64)
  ret = mbedtls_mpi_inv_mod(&d_inv, d1, &grp->N);
  if (ret!=0)
    goto cleanup;

  // t_i1 = d_inv * k1 * s2
  ret = mbedtls_mpi_mul_mpi(&t_i1, &d_inv, k1);
  if (ret!=0) {
    goto cleanup;
  }

  ret = mbedtls_mpi_mul_mpi(&t_i1, &t_i1, s2);
  if (ret!=0) {
    goto cleanup;
  }

  // t_i2 = d_inv * s3
  ret = mbedtls_mpi_mul_mpi(&t_i2, &d_inv, s3);
  if (ret!=0) {
    goto cleanup;
  }

  // t_i2 = t_i2 + t_i1
  ret = mbedtls_mpi_add_mpi(&t_i2, &t_i2, &t_i1);
  if (ret!=0) {
    goto cleanup;
  }

  // t_i2 = t_i2 - r
  ret = mbedtls_mpi_sub_mpi(&t_i2, &t_i2, r);
  if (ret!=0) {
    goto cleanup;
  }

  //s = t_i2 mod n
  ret = mbedtls_mpi_mod_mpi(s, &t_i2, &grp->N);
  if (ret!=0) {
    goto cleanup;
  }

  //s = 0 or s + r = n
  ret = mbedtls_mpi_add_mpi(&t_i2, s, r);
  if (ret!=0) {
    goto cleanup;
  }

  if( mbedtls_mpi_cmp_int( s, 0 ) == 0 || mbedtls_mpi_cmp_mpi(&t_i2, &grp->N)==0)
  {
    ret = -2;
    goto cleanup;
  }

  ret = 0;
cleanup:

  mbedtls_mpi_free(&d_inv);
  mbedtls_mpi_free(&t_i1);
  mbedtls_mpi_free(&t_i2);

  return ret;
}

int mbedtls_ssm2_sign(mbedtls_sm2_context *ctxc, mbedtls_sm2_context *ctxs,
    mbedtls_md_type_t md_alg,
    const unsigned char *hash, unsigned char *sig,
    int (*f_rng)(void *, unsigned char *, size_t), void *p_rng)
{
  int ret = 0;
  mbedtls_mpi k1;
  mbedtls_mpi e;
  mbedtls_mpi r;
  mbedtls_mpi s;
  mbedtls_mpi s2;
  mbedtls_mpi s3;
  mbedtls_ecp_point p1;

  mbedtls_mpi_init(&e);
  mbedtls_mpi_init(&k1);
  mbedtls_mpi_init(&r);
  mbedtls_mpi_init(&s);
  mbedtls_mpi_init(&s2);
  mbedtls_mpi_init(&s3);
  mbedtls_ecp_point_init(&p1);

  /**
   * A1: M' = Z || M
   * A2: e = Hash(M')
   * Parameter <hash> is the digest of <M'>, need convert to bignum <e>.
   */
  MBEDTLS_MPI_CHK(mbedtls_mpi_read_binary(&e, hash,
        mbedtls_md_get_size(mbedtls_md_info_from_type(md_alg))));

  do
  {
    // 签名第一步，客户端得到 p1, 送给服务器
    ret = mbedtls_ssm2_sign_step1(&ctxc->grp, &k1, &p1, f_rng, p_rng);
    if (ret!=0)
      goto cleanup;
    //得到k1, R1

    //p1, e 公开
    ret = mbedtls_ssm2_sign_step2(
        &ctxs->grp,
        &ctxs->d, // 服务器端私钥成分
        &p1,      // 客户端签名结果, R1
        &e,       // 客户端摘要
        &r,
        &s2,
        &s3,
        f_rng,
        p_rng);
    if (ret!=0)
      goto cleanup;

    ret = mbedtls_ssm2_sign_step3( &ctxc->grp, &ctxc->d, &k1, &r, &s2, &s3, &s);
    if (ret == -2)
    {
      continue;
    }

    break;
  }
  while (1);

  //MBEDTLS_MPI_CHK( ecdsa_signature_to_asn1( &r, &s, sig, slen ) );
  MBEDTLS_MPI_CHK(mbedtls_mpi_write_binary(&r, sig, mbedtls_mpi_size(&r)));
  MBEDTLS_MPI_CHK(mbedtls_mpi_write_binary(&s, sig + (ctxc->grp.nbits + 7) / 8, mbedtls_mpi_size(&s)));

cleanup:
  mbedtls_mpi_free(&e);
  mbedtls_mpi_free(&k1);
  mbedtls_mpi_free(&r);
  mbedtls_mpi_free(&s);
  mbedtls_mpi_free(&s2);
  mbedtls_mpi_free(&s3);
  mbedtls_ecp_point_free(&p1);

  return (ret);
}

/* ssm2 sign}  */

/* ssm2 genkey { */
/*
## 协同密钥的产生
1. 通讯A端生成随机数d1∈[1，n-1]，计算P1 = d1[*]G，作为子公钥1上传通讯B端。
2. 通讯B端生成随机数d2∈[1，n-1]，计算P = d2-1[*]P1[-]G，并作为公钥分发。
3. 恢复私钥
P = (d2^-1) * P1 [-] G
= (d2^-1) * (d1 * G) - G
= (d2^-1) * d1 * G - 1 * G
= ((d2^-1) * d1 - 1 ) * G
*/

/*
 ** generate d1, P1
 */
int mbedtls_ssm2_genkey_part1(mbedtls_ecp_group *grp,
    const mbedtls_ecp_point *G,
    mbedtls_mpi *d, mbedtls_ecp_point *Q,
    int (*f_rng)(void *, unsigned char *, size_t),
    void *p_rng)
{
  int ret;

  // d1∈[1，n-1]
  MBEDTLS_MPI_CHK( sm2_get_rand(grp, d, f_rng, p_rng ) );

  // p1＝d1[*]G
  // Multiplication R = m * P
  ret =  mbedtls_ecp_mul( grp, Q, d, G, f_rng, p_rng );
cleanup:
  return ret;
}

/*
 ** generate d2, P   d2=d, Q=p
 ** intput Q1 is P1
 */
int mbedtls_ssm2_genkey_part2(mbedtls_ecp_group *grp,
    const mbedtls_ecp_point *G,
    const mbedtls_ecp_point *Q1,
    mbedtls_mpi *d, mbedtls_ecp_point *Q,
    int (*f_rng)(void *, unsigned char *, size_t),
    void *p_rng)
{
  int ret;
  mbedtls_mpi d_inv, d_one;

  mbedtls_mpi_init(&d_inv);
  mbedtls_mpi_init(&d_one);

  // d1∈[1，n-1]
  MBEDTLS_MPI_CHK( sm2_get_rand(grp, d, f_rng, p_rng ) );

  // p＝d2^-1[*]p1[-]G = d_inv[*]p1[-]G
  // d_inv = d2^-1
  //* Modular inverse: X = A^-1 mod N  (HAC 14.61 / 14.64)
  ret = mbedtls_mpi_inv_mod(&d_inv, d, &grp->N);
  if(ret!=0)
    goto cleanup;

  // points by integers: R = m * P + n * Q
  // int mbedtls_ecp_muladd( mbedtls_ecp_group *grp, mbedtls_ecp_point *R,
  //           const mbedtls_mpi *m, const mbedtls_ecp_point *P,
  //           const mbedtls_mpi *n, const mbedtls_ecp_point *Q );
  // d_inv [*] G [-] G

  mbedtls_mpi_lset(&d_one, -1);
  ret = mbedtls_ecp_muladd( grp, Q, &d_inv, Q1, &d_one, G );

cleanup:
  mbedtls_mpi_free(&d_inv);
  mbedtls_mpi_free(&d_one);
  return ret;
}

int mbedtls_ssm2_recoverkey(const mbedtls_ecp_group *grp, mbedtls_mpi *d,
    const mbedtls_mpi *d1, const mbedtls_mpi *d2)
{
  int ret;
  mbedtls_mpi inv;

  //  = ((d2^-1) * d1 - 1 ) * G
  mbedtls_mpi_init(&inv);

  ret = mbedtls_mpi_inv_mod(&inv, d2, &grp->N);
  if (ret!=0)
    goto cleanup;

  ret = mbedtls_mpi_mul_mpi(&inv, &inv, d1);
  if (ret!=0)
    goto cleanup;

  ret = mbedtls_mpi_add_int(d, &inv, -1);
  if (ret!=0)
    goto cleanup;

  ret = mbedtls_mpi_mod_mpi(d, d, &grp->N);

cleanup:
  mbedtls_mpi_free(&inv);
  return ret;
}

int mbedtls_ssm2_check_pub_priv( const mbedtls_ecp_group *grp, const mbedtls_ecp_point *Q, const mbedtls_mpi *d )
{
  int ret = 0;
  mbedtls_ecp_point P;
  mbedtls_ecp_point_init(&P);

  /* Also checks d is valid */
  MBEDTLS_MPI_CHK( mbedtls_ecp_mul( (mbedtls_ecp_group*)grp, &P, d, &grp->G, NULL, NULL ) );

  if( mbedtls_mpi_cmp_mpi( &Q->X, &P.X ) ||
      mbedtls_mpi_cmp_mpi( &Q->Y, &P.Y ) ||
      mbedtls_mpi_cmp_mpi( &Q->Z, &P.Z ) )
  {
    ret = MBEDTLS_ERR_ECP_BAD_INPUT_DATA;
    goto cleanup;
  }

cleanup:
  mbedtls_ecp_point_free( &P );
  return( ret );
}

int mbedtls_ssm2_genkey(mbedtls_sm2_context *ctxc,
    mbedtls_sm2_context *ctxs,
    mbedtls_ecp_group_id gid,
    int (*f_rng)(void *, unsigned char *, size_t),
    void *p_rng)
{
  int ret = 0;

  // 载入曲线参数
  ret = mbedtls_ecp_group_load(&ctxc->grp, gid) ||
    mbedtls_ecp_group_load(&ctxs->grp, gid);
  if (ret!=0){
    printf("mbedtls_ecp_group_load fail");
    goto cleanup;
  }

  // 生成客户端的私钥成分及公钥 Step1
  ret = mbedtls_ssm2_genkey_part1(&ctxc->grp, &ctxc->grp.G, &ctxc->d, &ctxc->Q, f_rng, p_rng);
  if(ret!=0)
    goto cleanup;

  // 生成服务器端的私钥及公共公钥 Step2
  ret = mbedtls_ssm2_genkey_part2(&ctxs->grp, &ctxs->grp.G, &ctxc->Q,
      &ctxs->d, &ctxs->Q, f_rng, p_rng);
  if(ret!=0)
    goto cleanup;
  ret = mbedtls_ecp_check_pubkey( &ctxs->grp, &ctxs->Q );
  if (ret!=0)
    goto cleanup;

  ret = mbedtls_ecp_copy(&ctxc->Q, &ctxs->Q);
cleanup:
  return ret;
}
/* } ssm2 genkey */

int mbedtls_sm2_compute_key(
    mbedtls_ecp_group *grp,
    const mbedtls_mpi *rA,           //本端临时私钥 rA or rB
    const mbedtls_ecp_point *Ra,     //本端临时公钥 Ra or Rb  x1bar
    const mbedtls_mpi *dA,           //本端加密私钥 dA or dB
    const mbedtls_ecp_point *Pa,     //对端加密公钥 Pa or Pb
    const mbedtls_ecp_point *Rb,     //对端临时公钥 Rb or Ra
    const mbedtls_ecp_point *Pb,     //对端加密公钥 Pb or Pa,
    mbedtls_ecp_point *skey);
/*
   Pa, Pb 和 dA, dB 分别是 A 和 B 的公钥和私钥
   Za, Zb 分别表示 A 和 B 的唯一ID

   1. rA, rB 分别为A，B的随机数
   Ra = rA*G = (x2, y2)  发送给B
   Rb = rB*G = (x3, y3)  发送给A

   A 采用分割密钥， B为完整密钥
   */
/* ssm2 exchange key { */
/* UA 的客户度执行 */
int mbedtls_ssm2_computekey_step1(
    mbedtls_ecp_group *grp,
    const mbedtls_mpi *d1,
    mbedtls_mpi *k1,
    mbedtls_mpi *R1,
    int (*f_rng)(void *, unsigned char *, size_t),
    void *p_rng)
{
  int ret;

  // d1∈[1，n-1]
  MBEDTLS_MPI_CHK( sm2_get_rand(grp, k1, f_rng, p_rng ) );

  MBEDTLS_MPI_CHK( mbedtls_mpi_mul_mpi(R1, k1, d1 ) );

cleanup:
  return ret;
}

/* R 为用于交互密钥的临时公钥，对应的临时密钥成分为 d1, d2 */
/* UA 的服务端执行 */
int mbedtls_ssm2_computekey_step2(
    mbedtls_ecp_group *grp,
    const mbedtls_mpi *d2,
    const mbedtls_mpi *R1,
    mbedtls_mpi *k2,
    mbedtls_ecp_point *R,
    int (*f_rng)(void *, unsigned char *, size_t),
    void *p_rng)
{
  int ret;

  mbedtls_mpi t;
  mbedtls_mpi_init(&t);

  // d1∈[1，n-1]
  MBEDTLS_MPI_CHK( sm2_get_rand(grp, k2, f_rng, p_rng ) );
  MBEDTLS_MPI_CHK( mbedtls_mpi_inv_mod(&t, d2, &grp->N) );    //d2^-1
  MBEDTLS_MPI_CHK( mbedtls_mpi_mul_mpi(&t, &t, R1 ) );
  MBEDTLS_MPI_CHK( mbedtls_mpi_mul_mpi(&t, &t, k2 ) );
  MBEDTLS_MPI_CHK( mbedtls_mpi_mod_mpi(&t, &t, &grp->N ) );
  MBEDTLS_MPI_CHK( mbedtls_ecp_mul(grp, R, &t, &grp->G, NULL, NULL));

cleanup:
  mbedtls_mpi_free(&t);
  return ret;
}

/* UA客户端执行 用于计算 x1bar 和 x2bar */
int mbedtls_ssm2_computekey_step3(
    mbedtls_ecp_group *grp,
    const mbedtls_ecp_point *Ra,    //本端临时公钥 Ra or Rb  x1bar
//    const mbedtls_ecp_point *Pa,    //本端加密公钥 Pa or Pb
    const mbedtls_ecp_point *Rb,    //对端临时公钥 Rb or Ra
//    const mbedtls_ecp_point *Pb,    //对端加密公钥 Pb or Pa
    mbedtls_mpi* x1bar,
    mbedtls_mpi* x2bar)
{
  int ret;
  int w;
  mbedtls_mpi two_pow_w;
  mbedtls_mpi one;
  mbedtls_mpi_init(&two_pow_w);
  mbedtls_mpi_init(&one);

  mbedtls_mpi_lset(&one, 1);
  /*
   * w = ceil(keybits / 2) - 1
   * x = 2^w + (x and (2^w - 1)) = 2^w + (x mod 2^w)
   */
  w = (mbedtls_mpi_size(&grp->N) + 1) / 2 - 1;            //向上取整 w = 127
  mbedtls_mpi_lset(&two_pow_w, 1);
  ret = mbedtls_mpi_shift_l(&two_pow_w, w);               //two_pow_w = 2^w
  if (ret!=0)
  {
    goto cleanup;
  }

  //Ra x1bar
  //Rb x2bar
  //Caculate self x1bar
  ret = mbedtls_mpi_mod_mpi(x1bar, &Ra->X, &two_pow_w);
  if (ret!=0)
  {
    goto cleanup;
  }
  ret = mbedtls_mpi_add_mpi(x1bar, x1bar, &two_pow_w);
  if (ret!=0)
  {
    goto cleanup;
  }

  //Caculate peer x2bar
  ret = mbedtls_mpi_mod_mpi(x2bar, &Rb->X, &two_pow_w);
  if (ret!=0)
  {
    goto cleanup;
  }
  ret = mbedtls_mpi_add_mpi(x2bar, x2bar, &two_pow_w);
  if(ret!=0)
  {
    goto cleanup;
  }
cleanup:
  mbedtls_mpi_free(&two_pow_w);
  mbedtls_mpi_free(&one);
  return ret;
}

/* UA 客户端执行
 * 用到 临时密钥成分 r1 和 x1bar
 * 计算结果送到 UA 服务端
 **/
int mbedtls_ssm2_computekey_step4(
    const mbedtls_mpi* r1,
    const mbedtls_mpi* x1bar,
    mbedtls_mpi* i)
{
  return mbedtls_mpi_mul_mpi(i, r1, x1bar);     // xx1*r1
}

/* UA 服务端执行 */
int mbedtls_ssm2_computekey_step5(
    mbedtls_ecp_group *grp,
    const mbedtls_mpi* r2,
    const mbedtls_mpi* dA2,
    const mbedtls_mpi* i_from_client,
    mbedtls_mpi* i)
{
  int ret;
  mbedtls_mpi dd2;
  mbedtls_mpi_init(&dd2);

  MBEDTLS_MPI_CHK(mbedtls_mpi_inv_mod(&dd2, dA2, &grp->N)); // d2^-1
  MBEDTLS_MPI_CHK(mbedtls_mpi_mul_mpi(i, i_from_client, r2));         // 2a. xx1*r1*r2
  MBEDTLS_MPI_CHK(mbedtls_mpi_add_int(i, i, 1));          // 2b
  MBEDTLS_MPI_CHK(mbedtls_mpi_mul_mpi(i, i, &dd2));       // 2c
cleanup:
  mbedtls_mpi_free(&dd2);
  return ret;
}

/* UA 客户端执行,得到t */
int mbedtls_ssm2_computekey_step6(
    mbedtls_ecp_group *grp,
    const mbedtls_mpi *dA1,
    const mbedtls_mpi *i_from_server,
    mbedtls_mpi *t)
{
  int ret;
  MBEDTLS_MPI_CHK(mbedtls_mpi_mul_mpi(t, i_from_server, dA1));
  MBEDTLS_MPI_CHK(mbedtls_mpi_sub_int(t, t, 1));

  MBEDTLS_MPI_CHK(mbedtls_mpi_mod_mpi(t, t, &grp->N));
cleanup:
  return ret;
}

int ssm2_compute_key(
    mbedtls_ecp_group *grp,
    const mbedtls_mpi *r1,          //本端临时私钥 rA or rB
    const mbedtls_mpi *r2,
    const mbedtls_ecp_point *Ra,    //本端临时公钥 Ra or Rb  x1bar
    const mbedtls_mpi *dA1,         //本端加密私钥 dA or dB
    const mbedtls_mpi *dA2,         //本端加密私钥 dA or dB
    const mbedtls_ecp_point *Pa,    //本端加密公钥 Pa or Pb
    const mbedtls_ecp_point *Rb,    //对端临时公钥 Rb or Ra
    const mbedtls_ecp_point *Pb,    //对端加密公钥 Pb or Pa
    mbedtls_ecp_point *skey)
{
  int ret;
  mbedtls_mpi x1bar;
  mbedtls_mpi x2bar;
  mbedtls_mpi t;
  mbedtls_mpi one;

  mbedtls_mpi_init(&x1bar);
  mbedtls_mpi_init(&x2bar);
  mbedtls_mpi_init(&t);
  mbedtls_mpi_init(&one);

  mbedtls_mpi_lset(&one, 1);

  if (mbedtls_ecp_check_pubkey(grp, Ra)!=0 ||
      mbedtls_ecp_check_pubkey(grp, Pa)!=0 ||
      mbedtls_ecp_check_pubkey(grp, Rb)!=0 ||
      mbedtls_ecp_check_pubkey(grp, Pb)!=0)
  {
    ret = -1;
    goto cleanup;
  }

//  ret = mbedtls_ssm2_computekey_step3(grp, Ra, Pa, Rb, Pb, &x1bar, &x2bar);
  ret = mbedtls_ssm2_computekey_step3(grp, Ra, Rb, &x1bar, &x2bar);
  if(ret!=0) {
    ret = -1;
    goto cleanup;
  }


  /*Caculate t
    tA = (dA + xx1*rA) mod n
    = ((d1*d2^-1-1) + xx1*(R1*k2*d2^-1)) mod n
    = ((d1*d2^-1-1) + xx1*(k1*d1*k2*d2^-1)) mod n
    = (d1*d2^-1 + xx1 * d1*(d2^-1)*k1*k2 -1 ) mod n
    = (d1*(d2^-1 + xx1*(d2^-1)*k1*k2) -1 ) mod n
    = (d1*(d2^-1*(1+xx1*k1*k2)) -1 ) mod n

    t计算
    1. A端计算k1*xx1,发送给B端
    2. B端计算
    2a. 计算 k1*xx1*k2
    2b. 计算 k1*xx1*k2 + 1
    2c. 计算 t1 = d2^-1 * (k1*xx1*k2 + 1) , 发送给A端
    3. A计算 (t1*d1 -1) mod n
    */

  //A 端计算,结果i送到B端
  ret = mbedtls_ssm2_computekey_step4(r1, &x1bar, &t);  // xx1*r1
  if(ret!=0)
  {
    goto cleanup;
  }

  //B 端计算
  ret = mbedtls_ssm2_computekey_step5(grp, r2, dA2, &t, &t);
  if(ret!=0)
  {
    goto cleanup;
  }

  //A 端计算,得到t
  ret = mbedtls_ssm2_computekey_step6(grp, dA1, &t, &t);
  if (ret!=0)
  {
    goto cleanup;
  }

  if(1)
  {
    printf("\n");
    dump_mpi("t:", &t);
    printf("\n");
  }
  //Caculate U or V
  //[h * t](P + xbar * R) = (x, y)
  /*
     ret = mbedtls_mpi_mul_mpi(&t, &t, &h);
     if (ret!=0)
     {
     goto cleanup;
     }
     */
  /* UA 客户端执行，得到协同密钥 */
  ret = mbedtls_ecp_muladd(grp, skey, &one, Pb, &x2bar, Rb);
  if (ret!=0)
  {
    goto cleanup;
  }
  ret = mbedtls_ecp_mul(grp, skey, &t, skey, NULL, NULL);
  if (ret!=0)
  {
    goto cleanup;
  }
  if (mbedtls_mpi_cmp_int(&skey->Z, 0)==0)
  {
    ret = -1;
    goto cleanup;
  }

  if(1)
  {
    printf("\n");
    dump_point("pnt:", grp, skey);
    printf("\n");
  }
  /*
     if(is_server)
     {
  //Caculate ZA
  get_z(id, id_len, self_enc_pubkey, self_enc_pubkey_len, &pxyzab[65]);
  //Caculate ZB
  get_z(id, id_len, peer_enc_pubkey, peer_enc_pubkey_len, &pxyzab[97]);
  }
  else
  {
  //Caculate ZA
  get_z(id, id_len, peer_enc_pubkey, peer_enc_pubkey_len, &pxyzab[65]);
  //Caculate ZB
  get_z(id, id_len, self_enc_pubkey, self_enc_pubkey_len, &pxyzab[97]);
  }
  MBEDTLS_MPI_CHK(mbedtls_sm2_pbkdf2(&md_ctx, pxyzab, 64,
  NULL, 0, 0, klen, skey));
  */
cleanup:
  mbedtls_mpi_free(&x1bar);
  mbedtls_mpi_free(&x2bar);
  mbedtls_mpi_free(&t);
  mbedtls_mpi_free(&one);
  return ret;
}
/* } ssm2 exchange key */

/* sm2 exchange key { */
/*
   if(is_server)
   {
//Caculate ZA
get_z(id, id_len, self_enc_pubkey, self_enc_pubkey_len, &pxyzab[65]);
//Caculate ZB
get_z(id, id_len, peer_enc_pubkey, peer_enc_pubkey_len, &pxyzab[97]);
}
else
{
//Caculate ZA
get_z(id, id_len, peer_enc_pubkey, peer_enc_pubkey_len, &pxyzab[65]);
//Caculate ZB
get_z(id, id_len, self_enc_pubkey, self_enc_pubkey_len, &pxyzab[97]);
}
MBEDTLS_MPI_CHK(mbedtls_sm2_pbkdf2(&md_ctx, pxyzab, 64,
NULL, 0, 0, klen, skey));
*/
/* } sm2 exchange key */

/* selftest { */
#if defined(MBEDTLS_SELF_TEST)

/*
 * SM2 test vectors from: GM/T 0003-2012 Chinese National Standard
 */

static const unsigned char sm2_test_plaintext[] =   /* "encryption standard" */
{
  0x65, 0x6E, 0x63, 0x72, 0x79, 0x70, 0x74, 0x69,
  0x6F, 0x6E, 0x20, 0x73, 0x74, 0x61, 0x6E, 0x64,
  0x61, 0x72, 0x64,
};

static const unsigned char sm2_test_messagetext[] =     /* message digest */
{
  0x6D, 0x65, 0x73, 0x73, 0x61, 0x67, 0x65, 0x20,
  0x64, 0x69, 0x67, 0x65, 0x73, 0x74,
};
static const char sm2_test_ID[] =              /* "ALICE123@YAHOO.COM" */
{
  0x41, 0x4C, 0x49, 0x43, 0x45, 0x31, 0x32, 0x33,
  0x40, 0x59, 0x41, 0x48, 0x4F, 0x4F, 0x2E, 0x43,
  0x4F, 0x4D,
};
static const unsigned char sm2_test_z[] =
{
  0xF4, 0xA3, 0x84, 0x89, 0xE3, 0x2B, 0x45, 0xB6,
  0xF8, 0x76, 0xE3, 0xAC, 0x21, 0x68, 0xCA, 0x39,
  0x23, 0x62, 0xDC, 0x8F, 0x23, 0x45, 0x9C, 0x1D,
  0x11, 0x46, 0xFC, 0x3D, 0xBF, 0xB7, 0xBC, 0x9A,
};
static const unsigned char sm2_test_md[] =
{
  0xB5, 0x24, 0xF5, 0x52, 0xCD, 0x82, 0xB8, 0xB0,
  0x28, 0x47, 0x6E, 0x00, 0x5C, 0x37, 0x7F, 0xB1,
  0x9A, 0x87, 0xE6, 0xFC, 0x68, 0x2D, 0x48, 0xBB,
  0x5D, 0x42, 0xE3, 0xD9, 0xB9, 0xEF, 0xFE, 0x76,
};

int mbedtls_ssm2_self_test(int verbose)
{
  int ret = 0;
  mbedtls_sm2_context ctxc, ctxs;
  unsigned char output[512];
  size_t olen;
  unsigned char plain[512];
  size_t plen;
  mbedtls_ecp_group_id gid = MBEDTLS_ECP_DP_SM2P256T1;
  mbedtls_entropy_context entropy;
  mbedtls_ctr_drbg_context ctr_drbg;
  const char *pers = "ssm2_genkey";

  mbedtls_mpi d;

  mbedtls_mpi_init(&d);
  mbedtls_sm2_init(&ctxc);
  mbedtls_sm2_init(&ctxs);

  mbedtls_ctr_drbg_init( &ctr_drbg );
  mbedtls_entropy_init( &entropy );

  /* gnekey { */
  mbedtls_printf( "  Random seed:");
  if( ( ret = mbedtls_ctr_drbg_seed( &ctr_drbg, mbedtls_entropy_func, &entropy,
          (const unsigned char *) pers,
          strlen( pers ) ) ) != 0 )
  {
    mbedtls_printf( " failed\n  ! mbedtls_ctr_drbg_seed returned %d\n", ret );
    goto cleanup;
  }else
    mbedtls_printf( " success\n  SSM2 genkey: " );

  if ((ret = mbedtls_ssm2_genkey(&ctxc, &ctxs, gid, mbedtls_ctr_drbg_random, &ctr_drbg)) !=0 )
  {
    mbedtls_printf( " failed\n  ! mbedtls_ssm2_genkey returned %d\n", ret );
    goto cleanup;
  }else{
    mbedtls_printf( " success\n" );
    dump_mpi   ( "  + Client PrivateKey: ", &ctxc.d);
    dump_pubkey( "  + Client Public key: ", &ctxc );
    dump_mpi   ( "  + Server PrivateKey: ", &ctxs.d);
    dump_pubkey( "  + Server Public key: ", &ctxs );
    mbedtls_printf( " \n  SSM2 Get Hash Z: " );
  }
  /* } gnekey */

  /* calc hash { */
  if ((ret = mbedtls_sm2_hash_z(&ctxc, MBEDTLS_MD_SM3,
          sm2_test_ID, sizeof(sm2_test_ID), output)) != 0)
  {
    goto cleanup;
  }
  if (verbose != 0)
  {
    mbedtls_printf("passed\n  SM2 Get hash: ");
  }

  if ((ret = mbedtls_sm2_hash_e(MBEDTLS_MD_SM3,
          sm2_test_z, sm2_test_messagetext,
          sizeof(sm2_test_messagetext), output)) != 0)
  {
    if (verbose != 0)
    {
      mbedtls_printf("failed\n");
    }
    goto cleanup;
  }

  if (verbose != 0)
  {
    mbedtls_printf("passed\n  SSM2 sign: ");
  }
  /* } calc hash */

  /* ssm2 sign { */
  if ((ret = mbedtls_ssm2_sign(&ctxc, &ctxs, MBEDTLS_MD_SM3, sm2_test_md, output,
          mbedtls_ctr_drbg_random, &ctr_drbg)) != 0)
  {
    if (verbose != 0)
    {
      mbedtls_printf( "failed\n" );
    }
    goto cleanup;
  }
  if (verbose != 0)
  {
    mbedtls_printf("passed\n  SM2 verify: ");
  }

  if(0){
    ret = mbedtls_ssm2_recoverkey(&ctxc.grp, &d, &ctxc.d, &ctxs.d);
    if(ret==0)
    {
      ret = mbedtls_ecp_check_privkey(&ctxc.grp, &d);
      mbedtls_printf("mbedtls_ecp_check_privkey result 0x%04x\n,", ret);
      if (ret==0)
      {
        ret = mbedtls_ssm2_check_pub_priv(&ctxc.grp, &ctxs.Q, &d);
        if (ret!=0)
          goto cleanup;
      }
    }
    mbedtls_mpi_copy(&ctxc.d, &d);

    ret = mbedtls_sm2_sign(&ctxc, MBEDTLS_MD_SM3, sm2_test_md, output,
        mbedtls_ctr_drbg_random, &ctr_drbg);
    if (ret!=0)
    {
      mbedtls_printf("sm2_sign fail");
      goto cleanup;
    }
  }

  if ((ret = mbedtls_sm2_verify(&ctxc, MBEDTLS_MD_SM3, sm2_test_md, output)) != 0)
  {
    if (verbose != 0)
    {
      mbedtls_printf( "failed\n" );
    }
    goto cleanup;
  }

  if (verbose != 0)
  {
    mbedtls_printf("passed\n");
  }
  /* } ssm2 sign */

  /* sm2 encrypt/decrypt { */
  mbedtls_printf("  SM2 Encrypt:");
  if ((ret = mbedtls_sm2_encrypt(&ctxc, MBEDTLS_MD_SM3,
          sm2_test_plaintext, sizeof(sm2_test_plaintext),
          output, &olen,
          mbedtls_ctr_drbg_random, &ctr_drbg)) != 0)
  {
    mbedtls_printf( "failed\n" );
    goto cleanup;
  }else
    mbedtls_printf( "passed\n" );

  mbedtls_printf("  SSM2 Decrypt:");
  plen = sizeof(plain);
  if ((ret = mbedtls_ssm2_decrypt(&ctxc, &ctxs, MBEDTLS_MD_SM3,
          output, olen,
          plain, &plen)) != 0)
  {
    if (verbose != 0)
    {
      mbedtls_printf( "failed\n" );
    }
    goto cleanup;
  }
  if (plen != sizeof(sm2_test_plaintext) || memcmp(plain, sm2_test_plaintext, sizeof(sm2_test_plaintext)) != 0)
  {
    if (verbose != 0)
    {
      mbedtls_printf( "check failed\n" );
    }
    ret = 1;
    goto cleanup;
  }

  if (verbose != 0)
  {
    mbedtls_printf("passed\n");
  }

  if(0){
    mbedtls_printf("  SSM2 recoverykey:");

    ret = mbedtls_ssm2_recoverkey(&ctxc.grp, &d, &ctxc.d, &ctxs.d);
    if(ret==0)
    {
      ret = mbedtls_ecp_check_privkey(&ctxc.grp, &d);
      if (ret==0)
      {
        ret = mbedtls_ssm2_check_pub_priv(&ctxc.grp, &ctxs.Q, &d);
        if (ret!=0)
        {
          mbedtls_printf(" check_pub_priv fail 0x%04x\n,", ret);
          goto cleanup;
        }
        mbedtls_printf(" passed\n");
      }else
      {
        mbedtls_printf("check_privkey fail 0x%04x\n,", ret);
        goto cleanup;
      }
    }else
      mbedtls_printf("  fail\n");
    mbedtls_mpi_copy(&ctxc.d, &d);

    mbedtls_printf(" SSM2 decrypt:");
    plen = sizeof(plain);
    if ((ret = mbedtls_ssm2_decrypt(&ctxc, &ctxs, MBEDTLS_MD_SM3,
            output, olen,
            plain, &plen)) != 0)
    {
      if (verbose != 0)
      {
        mbedtls_printf( "failed\n" );
      }
      goto cleanup;
    }
    if (plen != sizeof(sm2_test_plaintext) || memcmp(plain, sm2_test_plaintext, sizeof(sm2_test_plaintext)) != 0)
    {
      if (verbose != 0)
      {
        mbedtls_printf( "check failed\n" );
      }
      ret = 1;
      goto cleanup;
    }
    if (verbose != 0)
    {
      mbedtls_printf("passed\n");
    }
  }
  /* } sm2 encrypt/decrypt */

  /* ssm2 compute key { */
  mbedtls_printf("  SSM2 compute key: ");
  if(1){
    mbedtls_mpi rA1;         //本端临时私钥 rA or rB
    mbedtls_mpi rA2;
    mbedtls_mpi t;
    mbedtls_ecp_point Ra;    //本端临时公钥 Ra or Rb  x1bar

    mbedtls_mpi rB;
    mbedtls_ecp_point Rb;    //对端临时公钥 Rb or Ra
    mbedtls_ecp_point Sa;
    mbedtls_ecp_point Sb;

    mbedtls_sm2_context key; //

    mbedtls_mpi_init(&rA1);
    mbedtls_mpi_init(&rA2);
    mbedtls_mpi_init(&t);
    mbedtls_mpi_init(&rB);
    mbedtls_ecp_point_init(&Ra);
    mbedtls_ecp_point_init(&Rb);
    mbedtls_ecp_point_init(&Sa);
    mbedtls_ecp_point_init(&Sb);
    mbedtls_sm2_init(&key);

    // UB 生成key, rB, Rb
    MBEDTLS_MPI_CHK( mbedtls_sm2_genkey(&key, gid, mbedtls_ctr_drbg_random, &ctr_drbg) );
    MBEDTLS_MPI_CHK( sm2_get_rand(&key.grp, &rB, mbedtls_ctr_drbg_random, &ctr_drbg) );
    MBEDTLS_MPI_CHK( mbedtls_ecp_mul(&key.grp, &Rb, &rB, &ctxs.grp.G, NULL, NULL) );
    MBEDTLS_MPI_CHK( mbedtls_ssm2_check_pub_priv(&key.grp, &Rb, &rB));

    //UA 生成 rA1, rA2, Ra
    //rA1, rA2 分别是用户 A 的客户端和服务端密钥协同成分
    mbedtls_ssm2_computekey_step1(
        &ctxc.grp,
        &ctxc.d,
        &rA1,
        &t,
        mbedtls_ctr_drbg_random,
        &ctr_drbg);

    mbedtls_ssm2_computekey_step2(
        &ctxs.grp,
        &ctxs.d,
        &t,
        &rA2,
        &Ra,
        mbedtls_ctr_drbg_random,
        &ctr_drbg);

    ret = ssm2_compute_key(
        &ctxc.grp,
        &rA1,      //本端临时私钥 rA or rB
        &rA2,
        &Ra,      //本端临时公钥 Ra or Rb  x1bar
        &ctxc.d,  //本端加密私钥 dA or dB
        &ctxs.d,
        &ctxc.Q,  //本端加密公钥 Pa or Pb
        &Rb,      //对端临时公钥 Rb or Ra
        &key.Q,   //对端加密公钥 Pb or Pa
        &Sa);
    if (ret!=0)
    {
      mbedtls_printf("ssm2_compute_key failed 0x%0x\n", ret);
      goto cleanup_ssm2_compute_key;
    }

    ret = mbedtls_sm2_compute_key(
        &key.grp,
        &rB,     //本端临时私钥 rA or rB
        &Rb,     //本端临时公钥 Ra or Rb  x1bar
        &key.d,  //本端加密私钥 dA or dB
        &key.Q,  //本端加密公钥 Pa or Pb//
        &Ra,     //对端临时公钥 Rb or Ra
        &ctxc.Q, //对端加密公钥 Pb or Pa
        &Sb);
    if (ret!=0)
    {
      mbedtls_printf("sm2_compute_key failed 0x%0x\n", ret);
      goto cleanup_ssm2_compute_key;
    }
    if( mbedtls_ecp_point_cmp(&Sa, &Sb)==0)
    {
      mbedtls_printf(" passed\n");
    }else
    {
      printf("  fail\n");
    }
cleanup_ssm2_compute_key:
    mbedtls_mpi_free(&rA1);
    mbedtls_mpi_free(&rA2);
    mbedtls_mpi_free(&t);
    mbedtls_mpi_free(&rB);
    mbedtls_ecp_point_free(&Ra);
    mbedtls_ecp_point_free(&Rb);
    mbedtls_ecp_point_free(&Sa);
    mbedtls_ecp_point_free(&Sb);
    mbedtls_sm2_free(&key);
  }
  /* } ssm2 compute key */

  /* sm2 compute key { */
  {
    mbedtls_mpi rA;           //本端临时私钥 rA or rB
    mbedtls_mpi rB;
    mbedtls_ecp_point Ra;     //本端临时公钥 Ra or Rb  x1bar
    mbedtls_ecp_point Rb;     //对端临时公钥 Rb or Ra
    mbedtls_ecp_point Sa;
    mbedtls_ecp_point Sb;

    mbedtls_mpi_init(&rA);
    mbedtls_mpi_init(&rB);
    mbedtls_ecp_point_init(&Ra);
    mbedtls_ecp_point_init(&Rb);
    mbedtls_ecp_point_init(&Sa);
    mbedtls_ecp_point_init(&Sb);

    mbedtls_printf("  SM2 compute key: ");
    ret = mbedtls_ssm2_recoverkey(&ctxc.grp, &d, &ctxc.d, &ctxs.d);
    if(ret==0)
    {
      ret = mbedtls_ecp_check_privkey(&ctxc.grp, &d);
      if (ret==0)
      {
        ret = mbedtls_ssm2_check_pub_priv(&ctxc.grp, &ctxs.Q, &d);
        if(ret!=0){
          mbedtls_printf("mbedtls_ecp_check_pub_priv result 0x%04x\n,", ret);
          goto cleanup_compute_key;
        }
      }else
      {
        mbedtls_printf("mbedtls_ecp_check_privkey result 0x%04x\n,", ret);
        goto cleanup_compute_key;
      }
    }else
      goto cleanup_compute_key;
    mbedtls_mpi_copy(&ctxc.d, &d);

    ret = mbedtls_sm2_genkey(&ctxc, gid, mbedtls_ctr_drbg_random, &ctr_drbg);
    if (ret!=0 )
    {
      goto cleanup_compute_key;
    }
    ret = mbedtls_sm2_genkey(&ctxs, gid, mbedtls_ctr_drbg_random, &ctr_drbg);
    if (ret!=0 )
    {
      goto cleanup_compute_key;
    }

    // r ∈[1，n-1]
    MBEDTLS_MPI_CHK( sm2_get_rand(&ctxc.grp, &rA, mbedtls_ctr_drbg_random, &ctr_drbg) );
    MBEDTLS_MPI_CHK( sm2_get_rand(&ctxs.grp, &rB, mbedtls_ctr_drbg_random, &ctr_drbg) );

    // R = r*G
    MBEDTLS_MPI_CHK( mbedtls_ecp_mul(&ctxc.grp, &Ra, &rA, &ctxc.grp.G, NULL, NULL) );
    MBEDTLS_MPI_CHK( mbedtls_ecp_mul(&ctxs.grp, &Rb, &rB, &ctxs.grp.G, NULL, NULL) );

    ret = mbedtls_sm2_compute_key(
        &ctxc.grp,
        &rA,      //本端临时私钥 rA or rB
        &Ra,      //本端临时公钥 Ra or Rb  x1bar
        &ctxc.d,  //本端加密私钥 dA or dB
        &ctxc.Q,  //本端加密公钥 Pa or Pb
        &Rb,      //对端临时公钥 Rb or Ra
        &ctxs.Q,  //对端加密公钥 Pb or Pa
        &Sa);
    if (ret!=0)
    {
      mbedtls_printf("sm2_compute_key failed 0x%0x\n", ret);
      goto cleanup_compute_key;
    }
    ret = mbedtls_sm2_compute_key(
        &ctxs.grp,
        &rB,     //本端临时私钥 rA or rB
        &Rb,     //本端临时公钥 Ra or Rb  x1bar
        &ctxs.d, //本端加密私钥 dA or dB
        &ctxs.Q, //本端加密公钥 Pa or Pb
        &Ra,     //对端临时公钥 Rb or Ra
        &ctxc.Q, //对端加密公钥 Pb or Pa
        &Sb);
    if (ret!=0)
    {
      mbedtls_printf("sm2_compute_key failed 0x%0x\n", ret);
      goto cleanup_compute_key;
    }
    if(mbedtls_ecp_point_cmp(&Sa, &Sb)==0)
    {
      printf("passed\n");
    }else
    {
      printf("fail\n");
    }
cleanup_compute_key:
    mbedtls_mpi_free(&rA);
    mbedtls_mpi_free(&rB);
    mbedtls_ecp_point_free(&Ra);
    mbedtls_ecp_point_free(&Rb);
    mbedtls_ecp_point_free(&Sa);
    mbedtls_ecp_point_free(&Sb);
  }
  /* } sm2 compute key */
cleanup:
  mbedtls_mpi_free(&d);
  mbedtls_sm2_free(&ctxc);
  mbedtls_sm2_free(&ctxs);

  mbedtls_ctr_drbg_free( &ctr_drbg );
  mbedtls_entropy_free( &entropy );
  return ret;
}
#endif /* MBEDTLS_SELF_TEST */
/* } selftest */
#endif /* MBEDTLS_SM2_C */
